#include "YangsFingerRootsDetector.h"

#include <opencv2/imgproc.hpp>
#include <opencv2/video.hpp>
#include <opencv2/objdetect.hpp>
#include <opencv2/highgui.hpp>

#include <iostream>
#include <algorithm>

YangsFingerRootsDetector::YangsFingerRootsDetector(cv::Mat &img)
{
    this->img = img;
}

YangsFingerRootsDetector::~YangsFingerRootsDetector()
{
}

bool YangsFingerRootsDetector::detectLR()
{
    //TODO
    return true;
}

ObjectDetector *YangsFingerRootsDetector::detect()
{
    this->isLeftHand = this->detectLR();
    this->img.copyTo(this->detected_result);
    cv::cvtColor(this->img, this->gray_tmp, cv::COLOR_BGR2GRAY);
    //cv::blur(this->gray_tmp, this->gray_tmp, cv::Size(11, 11));
    cv::threshold(this->gray_tmp, this->gray_tmp, 28, 255, cv::THRESH_BINARY);
    cv::dilate(this->gray_tmp, this->gray_tmp, cv::Mat(), cv::Point2d(-1, -1), 2);
    cv::erode(this->gray_tmp, this->gray_tmp, cv::Mat(), cv::Point2d(-1, -1), 2);
    //----------------------------------------------------------------------------------------

    this->findFingerRoots(this->getCornerPoints());
    cv::circle(this->detected_result, cv::Point(this->fingerConner[0], this->fingerConner[1]), 7, cv::Scalar(0, 255, 0), 1, 8, 0);
    cv::circle(this->detected_result, cv::Point(this->fingerConner[2], this->fingerConner[3]), 7, cv::Scalar(0, 255, 0), 1, 8, 0);
    cv::circle(this->detected_result, cv::Point(this->fingerConner[4], this->fingerConner[5]), 7, cv::Scalar(0, 255, 0), 1, 8, 0);

    //debug
    cv::imshow("key points", this->detected_result);
    cv::waitKey(0);
    return this;
}

int YangsFingerRootsDetector::getDetectedArr(int *result)
{
    /*
    if (this->fingerConner[0] > this->fingerConner[1] || this->fingerConner[2] > this->fingerConner[3] || fingerConner[4] > fingerConner[5])
    {
        return 0;
    }
    */
    for (int i = 0; i < 6; i++)
    {
        result[i] = this->fingerConner[i];
    }

    return 1;
}

vector<cv::Point> YangsFingerRootsDetector::getCornerPoints()
{
    const int thresh[4] = {70, 85, 115, 125}; //70, 85, 115, 125

    cv::Mat palm, detectedCorner;
    cv::Mat normImage[4];
    vector<cv::Point> corners;

    palm = this->gray_tmp;
    //cv::erode(palm, palm, cv::Mat());
    //cv::dilate(palm, palm, cv::Mat());

    for (int i = 0; i < 4; i++)
    {
        normImage[0] = cv::Mat::zeros(palm.rows, palm.cols, CV_32FC1);
    }
    cv::cornerHarris(palm, detectedCorner, 12, 7, 0.04); //small corner
    cv::normalize(detectedCorner, normImage[0], 0, 255, cv::NORM_MINMAX, CV_32FC1, cv::Mat());
    cv::cornerHarris(palm, detectedCorner, 20, 11, 0.04); //middle corner
    cv::normalize(detectedCorner, normImage[1], 0, 255, cv::NORM_MINMAX, CV_32FC1, cv::Mat());
    cv::cornerHarris(palm, detectedCorner, 30, 19, 0.04); //big corner
    cv::normalize(detectedCorner, normImage[2], 0, 255, cv::NORM_MINMAX, CV_32FC1, cv::Mat());
    cv::cornerHarris(palm, detectedCorner, 50, 27, 0.04); //very big corner
    cv::normalize(detectedCorner, normImage[3], 0, 255, cv::NORM_MINMAX, CV_32FC1, cv::Mat());

    for (int k = 0; k < 4; k++)
        for (int j = 0; j < normImage[k].rows; j++)
        {
            for (int i = 0; i < normImage[k].cols; i++)
            {
                if ((int)normImage[k].at<float>(j, i) > thresh[k])
                {
                    corners.push_back(cv::Point(i, j));
                }
            }
        }
    return corners;
}

void YangsFingerRootsDetector::findFingerRoots(vector<cv::Point> candidator)
{
    cv::Point result[3];
    vector<cv::Point> candidator_filtered = candidator;

    //candidator_filtered = this->filterFingerRoots_position(candidator_filtered);
    candidator_filtered = this->filterFingerRoots_PCA(candidator_filtered);
    //candidator_filtered = this->filterFingerRoots_KMeans(candidator_filtered);
    //candidator_filtered = this->filterFingerRoots_distance(candidator_filtered);

    //debug
    this->draw_candidators(candidator_filtered, cv::Scalar(255, 0, 0));

    for (int i = 0; i < 3; i++)
    {
        result[i] = candidator_filtered[i];
    }
    //finger roots [x1,y1,x2,y2,x3,y3]
    this->fingerConner[0] = result[0].x;
    this->fingerConner[1] = result[0].y;
    this->fingerConner[2] = result[1].x;
    this->fingerConner[3] = result[1].y;
    this->fingerConner[4] = result[2].x;
    this->fingerConner[5] = result[2].y;
}

float YangsFingerRootsDetector::distance(cv::Point a, cv::Point b)
{
    return (a.x - b.x) * (a.x - b.x) + (a.y - b.y) * (a.y - b.y);
}

void YangsFingerRootsDetector::draw_candidators(vector<cv::Point> candidator, cv::Scalar color)
{
    for (vector<cv::Point>::iterator i = candidator.begin(); i != candidator.end(); i++)
    {
        cv::circle(this->detected_result, *i, 5, cv::Scalar(255, 0, 0), 2, 8, 0);
    }
}

vector<cv::Point> YangsFingerRootsDetector::filterFingerRoots_Template(vector<cv::Point> candidator)
{
    if (candidator.size() < 3)
        return candidator;

    const int w = 51, h = 51;
    const float threthod = 0.4;
    float sum = 0, diff = 0;
    vector<cv::Point> result;
    cv::Mat finger_hole_template = cv::Mat::ones(cv::Size(w, h), CV_8U);

    for (int i = 0; i < w; i++)
    {
        for (int j = 0; j < h; j++)
        {
            if (i > j && i < w - j)
                finger_hole_template.at<uchar>(j, i) = 0;
        }
    }
    cv::threshold(finger_hole_template, finger_hole_template, 0.5, 255, cv::THRESH_OTSU);

    for (vector<cv::Point>::iterator iter = candidator.begin(); iter != candidator.end(); iter++)
    {
        sum = 0;
        for (int i = 0; i < w; i++)
        {
            for (int j = 0; j < h; j++)
            {
                sum += finger_hole_template.at<uchar>(j, i) == this->gray_tmp.at<uchar>(j + iter->y - h / 2, i + iter->x - w / 2);
            }
        }
        sum = sum / (w * h);
        std::cout << sum << endl;
        if (sum > threthod)
            result.push_back(*iter);
    }

    return result;
}

vector<cv::Point> YangsFingerRootsDetector::filterFingerRoots_aggregate(vector<cv::Point> candidator)
{
    if (candidator.size() < 3)
        return candidator;

    const int threshold = 40;
    const int patch_col = (this->detected_result.rows + 63) / 64;
    const int patch_row = (this->detected_result.cols + 63) / 64;
    vector<cv::Point> patch[patch_row][patch_col];
    vector<cv::Point> result;

    for (int i = 0; i < patch_row; i++)
    {
        for (int j = 0; j < patch_col; j++)
        {
            patch[i][j] = vector<cv::Point>();
        }
    }

    for (vector<cv::Point>::iterator i = candidator.begin(); i != candidator.end(); i++)
    {
        int x = i->x / 64;
        int y = i->y / 64;
        patch[x][y].push_back(*i);
    }

    for (int i = 0; i < patch_row; i++)
    {
        for (int j = 0; j < patch_col; j++)
        {
            if (patch[i][j].size() < threshold)
            {
                continue;
            }
            for (vector<cv::Point>::iterator k = patch[i][j].begin(); k != patch[i][j].end(); k++)
            {
                result.push_back(*k);
            }
        }
    }

    return result;
}

vector<cv::Point> YangsFingerRootsDetector::filterFingerRoots_position(vector<cv::Point> candidator)
{
    if (candidator.size() < 3)
        return candidator;

    //TODO: this function only suits for Tongji dataset
    const int w = 800, h = 600;
    const float x_low_bound = w * 1 / 16.0;
    const float x_up_bound = w * 12 / 16.0;
    const float y_low_bound = h * 0 / 16.0;
    const float y_up_bound = h * 8 / 16.0;
    vector<cv::Point> result;

    for (vector<cv::Point>::const_iterator iter = candidator.begin(); iter != candidator.end(); iter++)
    {
        if (iter->x < x_low_bound || iter->x > x_up_bound || iter->y < y_low_bound || iter->y > y_up_bound)
            continue;
        else
        {
            result.push_back(*iter);
        }
    }
    return result;
}

vector<cv::Point> YangsFingerRootsDetector::filterFingerRoots_KMeans(vector<cv::Point> candidator)
{
    //cv::meanShift();
    //cv::groupRectangles_meanshift();
    if (candidator.size() < 3)
        return candidator;

    const int cluster_num = 4; //5
    cv::Mat labels, centers;
    vector<cv::Point2f> points;
    vector<cv::Point> result;
    for (vector<cv::Point>::iterator i = candidator.begin(); i != candidator.end(); i++)
    {
        points.push_back(*i);
    }

    cv::kmeans(points, cluster_num, labels, cv::TermCriteria(cv::TermCriteria::EPS + cv::TermCriteria::COUNT, 10, 1.0), 3, cv::KMEANS_PP_CENTERS, centers); //cv::KmeansFlags::KMEANS_PP_CENTERS
    for (int i = 0; i < centers.rows; i++)
    {
        cv::Point center_coordinate(centers.at<float>(i, 0), centers.at<float>(i, 1));
        result.push_back(center_coordinate);
    }

    return result;
}

vector<cv::Point> YangsFingerRootsDetector::filterFingerRoots_PCA(vector<cv::Point> candidator)
{
    if (candidator.size() < 3)
        return candidator;

    vector<cv::Point> result;
    cv::Mat data(cv::Size(2, candidator.size()), CV_32F);
    for (int i = 0; i < candidator.size(); i++)
    {
        data.at<float>(i, 0) = candidator[i].x;
        data.at<float>(i, 1) = candidator[i].y;
    }

    cv::PCA pca(data, cv::Mat(), CV_PCA_DATA_AS_ROW, 2); //cv::PCA::Flags::DATA_AS_ROW
    cv::Mat mean = pca.mean, eval = pca.eigenvalues, evec = pca.eigenvectors;
    cv::Mat projects = pca.project(data), mean_project = pca.project(mean);

    //debug
    cv::Point2f mean_point(mean.row(0)), evec_point0(evec.row(0)), evec_point1(evec.row(1));
    cv::circle(this->detected_result, mean_point, 7, cv::Scalar(0, 0, 255), 1, 8, 0);
    cv::line(this->detected_result, mean_point, 600 * evec_point0 + mean_point, cv::Scalar(0, 0, 128));
    cv::line(this->detected_result, mean_point, 600 * evec_point1 + mean_point, cv::Scalar(128, 0, 0));

    auto pca_absolute_filter = [](vector<cv::Point> &candidator, cv::Mat &projects, cv::Mat &mean_project) -> vector<cv::Point> {
        const int threshod_v0[2] = {205, -115}; //up_bound & low_bound
        const int threshod_v1[2] = {110, -110};
        int i = 0;
        vector<cv::Point> result;

        for (vector<cv::Point>::iterator iter = candidator.begin(); iter != candidator.end(); iter++)
        {
            float x_err = projects.at<float>(i, 0) - mean_project.at<float>(0, 0);
            float y_err = projects.at<float>(i, 1) - mean_project.at<float>(0, 1);
            if (x_err > threshod_v0[0] || x_err < threshod_v0[1] || y_err > threshod_v1[0] || y_err < threshod_v1[1])
            {
            }
            else
            {
                result.push_back(*iter);
            }
            i++;
            //debug
            //cout << x_err << ',' << y_err << endl;
        }
        return result;
    };
    auto pca_relative_filter = [](vector<cv::Point> &candidator, cv::Mat &projects, cv::Mat &mean_project) -> vector<cv::Point> {
        float max_dim1_distance = -999999, min_dim1_distance = 999999, max_dim2_distance = -999999, min_dim2_distance = 999999;
        //[d1_up_bound][d1_down_bound]
        //[d2_up_bound][d2_down_bound]
        const float reserve_percentage[2][2] = {{0.85f, 1}, {0.85f, 1}}; //{{0.85f, 0.95f}, {0.80f, 0.92f}};
        int i = 0;
        vector<cv::Point> result;
        for (int i = 0; i < candidator.size(); i++)
        {
            float dim1_distance = projects.at<float>(i, 0) - mean_project.at<float>(0, 0);
            float dim2_distance = projects.at<float>(i, 1) - mean_project.at<float>(0, 1);
            if (max_dim1_distance < dim1_distance)
                max_dim1_distance = dim1_distance;
            if (min_dim1_distance > dim1_distance)
                min_dim1_distance = dim1_distance;
            if (max_dim2_distance < dim2_distance)
                max_dim2_distance = dim2_distance;
            if (min_dim2_distance > dim2_distance)
                min_dim2_distance = dim2_distance;
        }
        for (vector<cv::Point>::iterator iter = candidator.begin(); iter != candidator.end(); ++iter)
        {
            float dim1_distance = projects.at<float>(i, 0) - mean_project.at<float>(0, 0);
            float dim2_distance = projects.at<float>(i, 1) - mean_project.at<float>(0, 1);
            if (dim1_distance < max_dim1_distance * reserve_percentage[0][0] && dim1_distance > min_dim1_distance * reserve_percentage[0][1] && dim2_distance < max_dim2_distance * reserve_percentage[1][0] && dim2_distance > min_dim2_distance * reserve_percentage[1][0])
            {
                result.push_back(*iter);
            }
            ++i;
        }
        return result;
    };

    result = pca_absolute_filter(candidator, projects, mean_project);

    return result;
}

vector<cv::Point> YangsFingerRootsDetector::filterFingerRoots_yvalue(vector<cv::Point> candidator)
{
    if (candidator.size() < 3)
        return candidator;

    vector<cv::Point> result;
    const int output_size = 4;
    //big root heap
    auto m_compare = [](cv::Point &x, cv::Point &y) -> bool {
        return x.y < y.y;
    };
    make_heap(candidator.begin(), candidator.end(), m_compare);
    //up order;this is a feature of heap-sorting
    sort_heap(candidator.begin(), candidator.end(), m_compare);
    for_each(candidator.begin(), candidator.begin() + output_size, [&result](cv::Point x) -> void {
        result.push_back(x);
    });
    return result;
}

vector<cv::Point> YangsFingerRootsDetector::filterFingerRoots_xvalue(vector<cv::Point> candidator)
{
    if (candidator.size() < 3)
        return candidator;

    //big root heap
    auto m_compare = [](cv::Point &x, cv::Point &y) -> bool {
        return x.x < y.x;
    };
    //remove right points
    make_heap(candidator.begin(), candidator.end(), m_compare);
    pop_heap(candidator.begin(), candidator.end(), m_compare);
    candidator.pop_back();

    return candidator;
}

vector<cv::Point> YangsFingerRootsDetector::filterFingerRoots_distance(vector<cv::Point> candidator)
{
    //right hand roots pre-knowledges
    // cv::Point f_m_p(210, 295);
    // cv::Point m_r_p(210, 220);
    // cv::Point r_l_p(240, 150);
    cv::Point f_m_p(420, 190);
    cv::Point m_r_p(505, 200);
    cv::Point r_l_p(605, 240);
    if (this->isLeftHand)
    {
        //left hand roots pre-knowledges
        // f_m_p = cv::Point(220, 185);
        // m_r_p = cv::Point(210, 260);
        // r_l_p = cv::Point(240, 330);
        f_m_p = cv::Point(480, 190);
        m_r_p = cv::Point(380, 215);
        r_l_p = cv::Point(285, 240);
    }
    // debug
    cv::circle(this->detected_result, f_m_p, 5, cv::Scalar(0, 0, 255), 2, 8, 0);
    cv::circle(this->detected_result, m_r_p, 5, cv::Scalar(0, 0, 255), 2, 8, 0);
    cv::circle(this->detected_result, r_l_p, 5, cv::Scalar(0, 0, 255), 2, 8, 0);

    auto erase_neigbour = [this](vector<cv::Point> &vec, const cv::Point p) -> void {
        const int threshod = 1024;
        auto i = vec.cbegin();
        while (i != vec.cend())
        {
            float d = this->distance(*i, p);
            if (d < threshod)
                i = vec.erase(i);
            else
            {
                i++;
            }
        }
    };

    auto simple_distance = [erase_neigbour, this](vector<cv::Point> candidator, cv::Point f_m_p, cv::Point m_r_p, cv::Point r_l_p) -> vector<cv::Point> {
        float first_middle_ = 999999;
        float middle_ring_ = 999999;
        float ring_little_ = 999999;
        int d[3] = {0};
        cv::Point result[3];
        vector<cv::Point> result_vector;
        vector<cv::Point>::iterator tmp_i;

        for (vector<cv::Point>::iterator i = candidator.begin(); i != candidator.end(); i++)
        {
            d[0] = distance(*i, f_m_p);
            if (d[0] < first_middle_)
            {
                result[0] = *i;
                first_middle_ = d[0];
                tmp_i = i;
            }
        }
        erase_neigbour(candidator, *tmp_i);
        for (vector<cv::Point>::iterator i = candidator.begin(); i != candidator.end(); i++)
        {
            d[1] = distance(*i, m_r_p);
            if (d[1] < middle_ring_)
            {
                result[1] = *i;
                middle_ring_ = d[1];
                tmp_i = i;
            }
        }
        erase_neigbour(candidator, *tmp_i);
        for (vector<cv::Point>::iterator i = candidator.begin(); i != candidator.end(); i++)
        {
            d[2] = distance(*i, r_l_p);
            if (d[2] < ring_little_)
            {
                result[2] = *i;
                ring_little_ = d[2];
                tmp_i = i;
            }
        }
        //erase_neigbour(candidator, tmp_i);

        result_vector.push_back(result[0]);
        result_vector.push_back(result[1]);
        result_vector.push_back(result[2]);

        return result_vector;
    };

    auto preknowledge_distantce = [erase_neigbour, this](vector<cv::Point> candidator, cv::Point f_m_p, cv::Point m_r_p, cv::Point r_l_p) -> vector<cv::Point> {
        //most right -> f_m;second right -> m_r;remained closest -> r_l
        vector<cv::Point> result_vector;
        float ring_little_ = 999999, d;
        cv::Point r_l;
        auto m_compare = [](const cv::Point &a, const cv::Point &b) -> bool {
            return a.x < b.x;
        };

        std::make_heap(candidator.begin(), candidator.end(), m_compare);
        result_vector.push_back(candidator[0]);
        erase_neigbour(candidator, candidator[0]);

        std::make_heap(candidator.begin(), candidator.end(), m_compare);
        result_vector.push_back(candidator[0]);
        erase_neigbour(candidator, candidator[0]);

        for (auto i = candidator.begin(); i < candidator.end(); ++i)
        {
            d = distance(*i, r_l_p);
            if (d < ring_little_)
            {
                r_l = *i;
                ring_little_ = d;
            }
        }
        result_vector.push_back(r_l);

        return result_vector;
    };
    return preknowledge_distantce(candidator, f_m_p, m_r_p, r_l_p);
}

vector<cv::Point> YangsFingerRootsDetector::filterFingerRoots_Tangent(vector<cv::Point> candidator)
{
    if (candidator.size() < 3)
        return candidator;

    //todo
    cv::Mat canditate_mat = cv::Mat::zeros(this->detected_result.rows, this->detected_result.cols, CV_8U);
    cv::Mat lines;

    for (vector<cv::Point>::iterator i = candidator.begin(); i != candidator.end(); i++)
    {
        canditate_mat.at<uchar>(i->y, i->x) = 255;
    }

    cv::HoughLines(canditate_mat, lines, 1, CV_PI / 180, 1);

    for (int i = 0; i < lines.size().height; i++)
    {
        float rho = lines.at<float>(i, 0), theta = lines.at<float>(i, 1);
        cv::Point pt1, pt2;
        double a = cos(theta), b = sin(theta);
        double x0 = a * rho, y0 = b * rho;
        pt1.x = cvRound(x0 + 1000 * (-b));
        pt1.y = cvRound(y0 + 1000 * (a));
        pt2.x = cvRound(x0 - 1000 * (-b));
        pt2.y = cvRound(y0 - 1000 * (a));
        cv::line(this->detected_result, pt1, pt2, cv::Scalar(55, 100, 195), 1);
    }
    return candidator;
}